#include "stage.hh"
#include "worldfile.hh"
#include "canvas.hh"
#include "texture_manager.hh"
using namespace Stg;

// speech bubble colors
static const Color BUBBLE_FILL( 1.0, 0.8, 0.8 );// light blue/grey
static const Color BUBBLE_BORDER( 0,0,0 );  // black
static const Color BUBBLE_TEXT( 0,0,0 ); // black

void Model::DrawSelected()
{
  glPushMatrix();
  
  glTranslatef( pose.x, pose.y, pose.z+0.01 ); // tiny Z offset raises rect above grid
  
  Pose gpose = GetGlobalPose();
  
  char buf[64];
  snprintf( buf, 63, "%s [%.2f %.2f %.2f %.2f]", 
				token.c_str(), gpose.x, gpose.y, gpose.z, rtod(gpose.a) );
  
  PushColor( 0,0,0,1 ); // text color black
  Gl::draw_string( 0.5,0.5,0.5, buf );
  
  glRotatef( rtod(pose.a), 0,0,1 );
  
  Gl::pose_shift( geom.pose );
  
  double dx = geom.size.x / 2.0 * 1.6;
  double dy = geom.size.y / 2.0 * 1.6;
  
  PopColor();

  PushColor( 0,1,0,0.4 ); // highlight color blue
  glRectf( -dx, -dy, dx, dy );
  PopColor();

  PushColor( 0,1,0,0.8 ); // highlight color blue
  glLineWidth( 1 );
  glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
  glRectf( -dx, -dy, dx, dy );
  glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
  PopColor();

  glPopMatrix();
}


void Model::DrawTrailFootprint()
{
  double darkness = 0;
  double fade = 0.5 / (double)(trail_length+1);
	
  PushColor( 0,0,0,1 ); // dummy pushL just saving the color
	
  FOR_EACH( it, trail )
	 {
		TrailItem& checkpoint = *it;
		 
		glPushMatrix();
		Pose pz = checkpoint.pose;

		Gl::pose_shift( pz );
		Gl::pose_shift( geom.pose );
		 		 
		darkness += fade;
		Color c = checkpoint.color;
		c.a = darkness;
		glColor4f( c.r, c.g, c.b, c.a );
		 
		blockgroup.DrawFootPrint( geom );
		 
		glPopMatrix();
    }
	
  PopColor();
}

void Model::DrawTrailBlocks()
{
  double timescale = 0.0000001;

  FOR_EACH( it, trail )
	 {
		TrailItem& checkpoint = *it;
		 
		glPushMatrix();
		Pose pz = checkpoint.pose;
		pz.z =  (world->sim_time - checkpoint.time) * timescale;
		 
		Gl::pose_shift( pz );
		Gl::pose_shift( geom.pose );
		 
		DrawBlocks();

		glPopMatrix();
    }
}

void Model::DrawTrailArrows()
{
  double dx = 0.2;
  double dy = 0.07;
  double timescale = 1e-7;
  
  PushColor( 0,0,0,1 ); // dummy push

  FOR_EACH( it, trail )
	 {
		TrailItem& checkpoint = *it;

		glPushMatrix();
		Pose pz = checkpoint.pose;
		// set the height proportional to age
		pz.z =  (world->sim_time - checkpoint.time) * timescale;
		
		Gl::pose_shift( pz );
		Gl::pose_shift( geom.pose );
		
		Color& c = checkpoint.color;
		glColor4f( c.r, c.g, c.b, c.a );
		
		glBegin( GL_TRIANGLES );
		glVertex3f( 0, -dy, 0);
		glVertex3f( dx, 0, 0 );
		glVertex3f( 0, +dy, 0 );
		glEnd();
		
		glPopMatrix();
	 }
  
  PopColor();
}

void Model::DrawOriginTree()
{
  DrawPose( GetGlobalPose() );  

  FOR_EACH( it, children )
    (*it)->DrawOriginTree();
}
 

void Model::DrawBlocksTree( )
{
  PushLocalCoords();

  FOR_EACH( it, children )
	 (*it)->DrawBlocksTree();

  DrawBlocks();  
  PopCoords();
}
  
void Model::DrawPose( Pose pose )
{
  PushColor( 0,0,0,1 );
  glPointSize( 4 );
  
  glBegin( GL_POINTS );
  glVertex3f( pose.x, pose.y, pose.z );
  glEnd();
  
  PopColor();  
}

void Model::DrawBlocks( )
{ 
  blockgroup.CallDisplayList( this );
}

void Model::DrawBoundingBoxTree()
{
  PushLocalCoords();
  
  FOR_EACH( it, children )
	 (*it)->DrawBoundingBoxTree();

  DrawBoundingBox();
  PopCoords();
}

void Model::DrawBoundingBox()
{
  Gl::pose_shift( geom.pose );  

  PushColor( color );
  
  glBegin( GL_QUAD_STRIP );
  
  glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, geom.size.z );
  glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, 0 );
 
  glVertex3f( +geom.size.x/2.0, -geom.size.y/2.0, geom.size.z );
  glVertex3f( +geom.size.x/2.0, -geom.size.y/2.0, 0 );
 
  glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, geom.size.z );
  glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, 0 );

  glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, geom.size.z );
  glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, 0 );

  glVertex3f( -geom.size.x/2.0, +geom.size.y/2.0, geom.size.z );
  glVertex3f( -geom.size.x/2.0, +geom.size.y/2.0, 0 );

  glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, geom.size.z );
  glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, 0 );

  glEnd();

  glBegin( GL_LINES );
  glVertex2f( -0.02, 0 ); 
  glVertex2f( +0.02, 0 ); 

  glVertex2f( 0, -0.02 ); 
  glVertex2f( 0, +0.02 ); 
  glEnd();

  PopColor();
}

// move into this model's local coordinate frame
void Model::PushLocalCoords()
{
  glPushMatrix();  
  
  if( parent )
    glTranslatef( 0,0, parent->geom.size.z );
  
  Gl::pose_shift( pose );
}

void Model::PopCoords()
{
  glPopMatrix();
}

void Model::AddVisualizer( Visualizer* cv, bool on_by_default )
{
  if( !cv )
	 return;
  
  // If there's no GUI, ignore this request
  if( ! world_gui ) 
	 return;
	
  //save visual instance
  cv_list.push_back( cv );
  
  //register option for all instances which share the same name
  Canvas* canvas = world_gui->GetCanvas();
  std::map< std::string, Option* >::iterator i = canvas->_custom_options.find( cv->GetMenuName() );
  if( i == canvas->_custom_options.end() ) {
	 Option* op = new Option( cv->GetMenuName(), 
									  cv->GetWorldfileName(), 
									  "", 
									  on_by_default, 
									  world_gui );
	 canvas->_custom_options[ cv->GetMenuName() ] = op;
	 RegisterOption( op );
  }
}

void Model::RemoveVisualizer( Visualizer* cv )
{
  if( cv )
	 EraseAll( cv, cv_list );
  
  //TODO unregister option - tricky because there might still be instances attached to different models which have the same name
}


void Model::DrawStatusTree( Camera* cam ) 
{
  PushLocalCoords();
  DrawStatus( cam );
  FOR_EACH( it, children )
	 (*it)->DrawStatusTree( cam );  
  PopCoords();
}

void Model::DrawStatus( Camera* cam ) 
{
  if( power_pack || !say_string.empty() )	  
    {
      float yaw, pitch;
      pitch = - cam->pitch();
      yaw = - cam->yaw();			
      
      Pose gpz = GetGlobalPose();
      
      float robotAngle = -rtod(gpz.a);
      glPushMatrix();
      
      // move above the robot
      glTranslatef( 0, 0, 0.5 );		
      
      // rotate to face screen
      glRotatef( robotAngle - yaw, 0,0,1 );
      glRotatef( -pitch, 1,0,0 );
      
      if( power_pack )
		  power_pack->Visualize( cam );
      
      if( !say_string.empty() )
		  {
			 glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
			 
			 //get raster positition, add gl_width, then project back to world coords
			 glRasterPos3f( 0, 0, 0 );
			 GLfloat pos[ 4 ];
			 glGetFloatv(GL_CURRENT_RASTER_POSITION, pos);
			 
			 GLboolean valid;
			 glGetBooleanv( GL_CURRENT_RASTER_POSITION_VALID, &valid );
	  
			 if( valid ) 
				{				  
				  //fl_font( FL_HELVETICA, 12 );
				  float w = gl_width( this->say_string.c_str() ); // scaled text width
				  float h = gl_height(); // scaled text height
				  
				  GLdouble wx, wy, wz;
				  GLint viewport[4];
				  glGetIntegerv(GL_VIEWPORT, viewport);
				  
				  GLdouble modelview[16];
				  glGetDoublev(GL_MODELVIEW_MATRIX, modelview);
				  
				  GLdouble projection[16];	
				  glGetDoublev(GL_PROJECTION_MATRIX, projection);
	      
				  //get width and height in world coords
				  gluUnProject( pos[0] + w, pos[1], pos[2], modelview, projection, viewport, &wx, &wy, &wz );
				  w = wx;
				  gluUnProject( pos[0], pos[1] + h, pos[2], modelview, projection, viewport, &wx, &wy, &wz );
				  h = wy;
	      
				  // calculate speech bubble margin
				  const float m = h/10;
	      
				  // draw inside of bubble
				  PushColor( BUBBLE_FILL );
				  glPushAttrib( GL_POLYGON_BIT | GL_LINE_BIT );
				  glPolygonMode( GL_FRONT, GL_FILL );
				  glEnable( GL_POLYGON_OFFSET_FILL );
				  glPolygonOffset( 1.0, 1.0 );
				  Gl::draw_octagon( w, h, m );
				  glDisable( GL_POLYGON_OFFSET_FILL );
				  PopColor();
	      
				  // draw outline of bubble
				  PushColor( BUBBLE_BORDER );
				  glLineWidth( 1 );
				  glEnable( GL_LINE_SMOOTH );
				  glPolygonMode( GL_FRONT, GL_LINE );
				  Gl::draw_octagon( w, h, m );
				  glPopAttrib();
				  PopColor();
	      
				  PushColor( BUBBLE_TEXT );
				  // draw text inside the bubble
				  Gl::draw_string( m, 2.5*m, 0, this->say_string.c_str() );
				  PopColor();			
				}
		  }
      glPopMatrix();
    }
  
  if( stall )
    {
      DrawImage( TextureManager::getInstance()._stall_texture_id, cam, 0.85 );
    }
  
  //   extern GLuint glowTex;
  //   extern GLuint checkTex;
  
  //   if( parent == NULL )
  //  	 {
  //  		glBlendFunc(GL_SRC_COLOR, GL_ONE );	
  //  		DrawImage( glowTex, cam, 1.0 );
  //  		glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA );
  //  	 }
}

void Model::DrawImage( uint32_t texture_id, Camera* cam, float alpha, 
							  double width, double height )
{
  float yaw, pitch;
  pitch = - cam->pitch();
  yaw = - cam->yaw();

  float robotAngle = -rtod( GetGlobalPose().a);

  glPolygonMode( GL_FRONT, GL_FILL );

  glEnable(GL_TEXTURE_2D);
  glBindTexture( GL_TEXTURE_2D, texture_id );

  glColor4f( 1.0, 1.0, 1.0, 1.0 );
  glPushMatrix();

  //position image above the robot
  // TODO 
  glTranslatef( 0.0, 0.0, ModelHeight() + 0.3 );

  // rotate to face screen
  glRotatef( robotAngle - yaw, 0,0,1 );
  glRotatef( -pitch - 90, 1,0,0 );

  //draw a square, with the textured image
  glBegin(GL_QUADS);
  glTexCoord2f(0.0f, 0.0f); glVertex3f(-0.25f, 0, -0.25f );
  glTexCoord2f(width, 0.0f); glVertex3f( 0.25f, 0, -0.25f );
  glTexCoord2f(width, height); glVertex3f( 0.25f, 0,  0.25f );
  glTexCoord2f(0.0f, height); glVertex3f(-0.25f, 0,  0.25f );
  glEnd();

  glBindTexture( GL_TEXTURE_2D, 0 );
  glDisable(GL_TEXTURE_2D);

  //   glPolygonMode( GL_FRONT, GL_LINE );
  //   glColor3f( 0,0,1 );
  //   glBegin(GL_QUADS);
  //   glVertex3f(-0.25f, 0, -0.25f );
  //   glVertex3f( 0.25f, 0, -0.25f );
  //   glVertex3f( 0.25f, 0,  0.25f );
  //   glVertex3f(-0.25f, 0,  0.25f );
  //   glEnd();

  glPopMatrix();
}


void Model::DrawFlagList( void )
{	
  if( flag_list.size() < 1 )
    return;
  
  PushLocalCoords();
  
  glPolygonMode( GL_FRONT, GL_FILL );

  GLUquadric* quadric = gluNewQuadric();
  glTranslatef(0,0,1); // jump up
  Pose gpose = GetGlobalPose();
  glRotatef( 180 + rtod(-gpose.a),0,0,1 );
  
  for( std::list<Flag*>::reverse_iterator it( flag_list.rbegin()); 
		 it != flag_list.rend(); 
		 it++ )
    {		
      Flag* flag = *it;

		glTranslatef( 0, 0, flag->size/2.0 );
		
		flag->Draw( quadric );

      glTranslatef( 0, 0, flag->size/2.0 );
    }
  
  
  gluDeleteQuadric( quadric );
  
  PopCoords();
}


// void Model::DrawBlinkenlights()
// {
//   PushLocalCoords();

//   GLUquadric* quadric = gluNewQuadric();
//   //glTranslatef(0,0,1); // jump up
//   //Pose gpose = GetGlobalPose();
//   //glRotatef( 180 + rtod(-gpose.a),0,0,1 );

//   for( unsigned int i=0; i<blinkenlights->len; i++ )
//     {
//       stg_blinkenlight_t* b = 
// 	(stg_blinkenlight_t*)g_ptr_array_index( blinkenlights, i );
//       assert(b);

//       glTranslatef( b->pose.x, b->pose.y, b->pose.z );

//       PushColor( b->color );

//       if( b->enabled )
// 		  gluQuadricDrawStyle( quadric, GLU_FILL );
//       else
// 		  gluQuadricDrawStyle( quadric, GLU_LINE );
		
//       gluSphere( quadric, b->size/2.0, 8,8  );

//       PopColor();
//     }

//   gluDeleteQuadric( quadric );

//   PopCoords();
// }

void Model::DrawPicker( void )
{
  //PRINT_DEBUG1( "Drawing %s", token );
  PushLocalCoords();

  // draw the boxes
  blockgroup.DrawSolid( geom );

  // recursively draw the tree below this model 
  FOR_EACH( it, children )
	 (*it)->DrawPicker();

  PopCoords();
}

void Model::DataVisualize( Camera* cam )
{  
}

void Model::DataVisualizeTree( Camera* cam )
{
  PushLocalCoords();

  if( subs > 0 )
	 {
		DataVisualize( cam ); // virtual function overridden by some model types  
		
		FOR_EACH( it, cv_list )
		  {
			 Visualizer* vis = *it;
			 if( world_gui->GetCanvas()->_custom_options[ vis->GetMenuName() ]->isEnabled() )
				vis->Visualize( this, cam );
		  }
	 }

  // and draw the children
  FOR_EACH( it, children )
	 (*it)->DataVisualizeTree( cam );

  PopCoords();
}

void Model::DrawGrid( void )
{
  if ( gui.grid ) 
    {
      PushLocalCoords();
		
      stg_bounds3d_t vol;
      vol.x.min = -geom.size.x/2.0;
      vol.x.max =  geom.size.x/2.0;
      vol.y.min = -geom.size.y/2.0;
      vol.y.max =  geom.size.y/2.0;
      vol.z.min = 0;
      vol.z.max = geom.size.z;
		 
      PushColor( 0,0,1,0.4 );
		Gl::draw_grid(vol);
      PopColor();		 
      PopCoords();
    }
}
